#include "adxl345.h"

void ADXL345_readAngle(short *xAngle, short *yAngle, short *zAngle, uint8_t times);

ADXL345ClassStruct ADXL345Class = {
    .init = ADXL345_init,
    .getAngle = ADXL345_readAngle
};

#define ADXL345_IIC_SDA     GPIO_NUM_27
#define ADXL345_IIC_SCL     GPIO_NUM_14
#define ADXL345_IIC_NUM     I2C_NUM_0

#define ADXL345_Address     0x53 // SDO接VCC 从机地址为0x1D  SDO接GND 从机地址为0x53

#define OFSX                0x1E
#define OFSY                0x1F
#define OFSZ                0x20
#define BW_RATE             0x2C
#define POWER_CTL           0x2D
#define INT_ENABLE          0x2E
#define DATA_FORMAT         0x31
#define DATAX0              0X32
#define DATAX1              0X33  //与DATAX0组成x轴输出数据(二进制补码),DATAX1为高位,4mg/Bit
#define DATAY0              0X34
#define DATAY1              0X35  //与DATAY0组成Y轴输出数据(二进制补码),DATAY1为高位,4mg/Bit
#define DATAZ0              0X36
#define DATAZ1              0X37  //与DATAZ0组成Z轴输出数据(二进制补码),DATAZ1为高位,4mg/Bit

static const char *TAG = "ADXL345";

/**
  * @brief  写寄存器
  * @param  addr: 寄存器地址
  * @param  val: 写入的值
  * @retval None
  * @note   None
  */
void ADXL345_WR_Reg(uint8_t addr,uint8_t val) 
{
    i2c_cmd_handle_t cmd_handle = i2c_cmd_link_create();
    i2c_master_start(cmd_handle);
	i2c_master_write_byte(cmd_handle, (ADXL345_Address << 1 | I2C_MASTER_WRITE), true);
	i2c_master_write_byte(cmd_handle, addr, true);
	i2c_master_write_byte(cmd_handle, val, true);
    i2c_master_stop(cmd_handle);
    i2c_master_cmd_begin(ADXL345_IIC_NUM, cmd_handle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd_handle);
}
/**
  * @brief  读寄存器
  * @param  addr: 寄存器地址
  * @retval 读到的值
  * @note   None
  */
uint8_t ADXL345_RD_Reg(uint8_t addr) 		
{
	uint8_t data = 0;
    i2c_cmd_handle_t cmd_handle = i2c_cmd_link_create();
    i2c_master_start(cmd_handle);
	i2c_master_write_byte(cmd_handle, (ADXL345_Address << 1 | I2C_MASTER_WRITE), true);
	i2c_master_write_byte(cmd_handle, addr, true);
    i2c_master_start(cmd_handle);
	i2c_master_write_byte(cmd_handle, (ADXL345_Address << 1 | I2C_MASTER_READ), true);
    i2c_master_read(cmd_handle, &data, 1, I2C_MASTER_NACK);
    i2c_master_stop(cmd_handle);
    i2c_master_cmd_begin(ADXL345_IIC_NUM, cmd_handle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd_handle);
	return data;
}
uint8_t ADXL345_checkDevice(void)
{
    if(ADXL345_RD_Reg(0x00) == 0xE5)
    {	
		ADXL345_WR_Reg(DATA_FORMAT, 0X0B);   // 低电平中断输出,13位全分辨率,输出数据右对齐,16g量程 
		ADXL345_WR_Reg(BW_RATE, 0x0E);       // 数据输出速度为100Hz
		ADXL345_WR_Reg(POWER_CTL,0x08);      // 链接使能,测量模式,省电特性
		ADXL345_WR_Reg(INT_ENABLE, 0x80);    // 不使用中断		 
	 	ADXL345_WR_Reg(OFSX, 0x00);
		ADXL345_WR_Reg(OFSY, 0x00);
		ADXL345_WR_Reg(OFSZ, 0x05);
        return 0;
	}
    return 1;
}
/**
  * @brief  读取三轴数据
  * @param  x: x轴数据buffer地址
  * @param  y: y轴数据buffer地址
  * @param  z: z轴数据buffer地址
  * @retval None
  */
void ADXL345_readData(short *x, short *y, short *z)
{
    uint8_t buffer[6];
    i2c_cmd_handle_t cmd_handle = i2c_cmd_link_create();
    i2c_master_start(cmd_handle);
    i2c_master_write_byte(cmd_handle, (ADXL345_Address << 1 | I2C_MASTER_WRITE), true);
    i2c_master_write_byte(cmd_handle, DATAX0, true);
    i2c_master_start(cmd_handle);
	i2c_master_write_byte(cmd_handle, (ADXL345_Address << 1 | I2C_MASTER_READ), true);
    i2c_master_read(cmd_handle, buffer, 6, I2C_MASTER_ACK);
    i2c_master_stop(cmd_handle);
    i2c_master_cmd_begin(ADXL345_IIC_NUM, cmd_handle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd_handle);
	*x=(short)(((uint16_t)buffer[1] << 8) + buffer[0]); // 合成数据
	*y=(short)(((uint16_t)buffer[3] << 8) + buffer[2]);
	*z=(short)(((uint16_t)buffer[5] << 8) + buffer[4]);
}

/**
  * @brief  连读读取几次取平均值
  * @param  None
  * @retval None
  */
void ADXL345_readAverage(short *x, short *y, short *z, uint8_t times)
{
	short tempX, tempY, tempZ;
	*x=0;
	*y=0;
	*z=0;
	if(times)//读取次数不为0
	{
		for(uint8_t i = 0; i < times; i++)//连续读取times次
		{
			ADXL345_readData(&tempX, &tempY, &tempZ);
			*x+=tempX;
			*y+=tempY;
			*z+=tempZ;
			vTaskDelay(5 / portTICK_PERIOD_MS);
		}
		*x /= times;
		*y /= times;
		*z /= times;
	}
}
/**
  * @brief  计算角度
  * @param  None
  * @retval None
  */
void ADXL345_readAngle(short *xAngle, short *yAngle, short *zAngle, uint8_t times)
{
	short ax,ay,az;
	ADXL345_readAverage(&ax,&ay,&az,times);
	*xAngle=atan(ax/sqrt((az*az+ay*ay)))*180/3.14;
	*yAngle=atan(ay/sqrt((ax*ax+az*az)))*180/3.14;
	*zAngle=atan(sqrt(az == 0 ? 0 : (ax*ax+ay*ay)/az))*180/3.14;
}

void ADXL345_run_task(void *pvParameters) 
{

	while(1) 
    {
        ADXL345_readAngle(&ADXL345Class.xAngle, &ADXL345Class.yAngle, &ADXL345Class.zAngle, 20);
        // printf("x: %d, y: %d, z: %d\n", ADXL345Class.xAngle, ADXL345Class.yAngle, ADXL345Class.zAngle);
		vTaskDelay(200 / portTICK_PERIOD_MS);
	}
}

void ADXL345_init(void) 
{
    vTaskDelay(5000 / portTICK_PERIOD_MS);

    i2c_config_t ADXL345_IIC_config = {
        .mode = I2C_MODE_MASTER,                    //I2C 模式
        .sda_io_num = ADXL345_IIC_SDA,              //SDA IO映射
        .sda_pullup_en = GPIO_PULLUP_ENABLE,       //SDA IO模式
        .scl_io_num = ADXL345_IIC_SCL,              //SCL IO映射
        .scl_pullup_en = GPIO_PULLUP_ENABLE,       //SCL IO模式
        .master.clk_speed = 200000                  //I2C CLK频率
    };

    i2c_param_config(ADXL345_IIC_NUM, &ADXL345_IIC_config);
    i2c_driver_install(ADXL345_IIC_NUM, ADXL345_IIC_config.mode, 0, 0, 0);
    // 读取器件ID
	while(ADXL345_checkDevice())
    {	
        ESP_LOGI(TAG, "ADXL345 check failed, delay 1s");
        vTaskDelay(500 / portTICK_PERIOD_MS);
	}
    ESP_LOGI(TAG, "ADXL345 init complate");

    xTaskCreate(&ADXL345_run_task, "ADXL345_run_task", 2048, NULL, 9, NULL);
}

